#include "Ap.h"
#include "EEPROMmanager.h"
#include "Aircraft.h"
#include <Arduino.h>

#define OFF 0

#define AP_MODE_STANDBY 0
#define AP_MODE_WPNAV 1
#define AP_MODE_INDV 2
#define AP_MODE_LAND 3

#define PID_COUNT 6

#define AP_MODE_INDV_ALL AP_MODE_INDV_HEADING | AP_MODE_INDV_ALT | AP_MODE_INDV_THRO | AP_MODE_INDV_SLIP

#define AP_MODE_INDV_MASK_ROLL 3
#define AP_MODE_INDV_WING_LEVELER 1
#define AP_MODE_INDV_HEADING 2

#define AP_MODE_INDV_MASK_PITCH  12
#define AP_MODE_INDV_PITCH 4
#define AP_MODE_INDV_VRATE 8
#define AP_MODE_INDV_ALT   12

#define AP_MODE_INDV_MASK_THRO 16
#define AP_MODE_INDV_THRO  16

#define AP_MODE_INDV_MASK_SLIP 32
#define AP_MODE_INDV_SLIP 32

Ap::Ap()
{
	SpidConstants pids;
	pids = EEPROMmanager::readPIDconstants();
	pid.rollHold.setFrom(pids.wingLeveler);
	pid.headingHold.setFrom(pids.headingHold);
	pid.pitchHold.setFrom(pids.pitchHold);
	pid.vrateHold.setFrom(pids.climbHold);
	pid.altHold.setFrom(pids.altHold);
	pid.throttleHold.setFrom(pids.throttleHold);
	pid.slipHold.setFrom(pids.slipHold);
}

void Ap::init(Aircraft *aircraft)
{
	this->aircraft = aircraft;
	
	//set all targets to default (mostly 0)
	pidTarget.roll = 0;
	pidTarget.heading = 0;
	pidTarget.pitch = 0;
	pidTarget.vrate = 0;
	pidTarget.altitude = 500; // feet
	pidTarget.speed = 30; // knots
	pidTarget.slip = 0;
	
	mode = AP_MODE_INDV; //AP_MODE_STANDBY; //@TODO: TEMPORARY
	indv_holds = AP_MODE_INDV_WING_LEVELER; //AP_MODE_INDV_ALL;
}

void Ap::run()
{
	//We need to calculate dt even if the autopilot is not on
	double dt = findDT();
	
	//Lets see if autopilot is on or not
	//If the throttle (CH3) is below 1000 the RC unit is reporting lost RC coms
	//If CH8 is above 1500, it is reporting autopilot on
	//Serial.println(aircraft->RCin(3));
	//if (aircraft->RCin(3)<1000 || aircraft->RCin(8)>1500)
	if (aircraft->RCin(8)<1500)
	{onRun(dt);}else{offRun();}
    //offRun();
}

double Ap::findDT()
{
	double dt;
	long t;
	t = millis();
	dt = (t-lastPidTime)/1000.0;
	lastPidTime = t;
	return dt;
}

void Ap::onRun(double dt)
{
	en = true;
	
	switch (mode)
	{
	case AP_MODE_STANDBY:
		break;
	case AP_MODE_WPNAV:
		runWpNav(dt); //@TODO: Not implemented yet
		break;
	case AP_MODE_INDV:
		runIndv(dt);
		break;
	case AP_MODE_LAND:
		runLand(dt);
		break;
	default:
		break;
	}
}

void Ap::runIndv(double dt)
{
	int rollChannel, pitchChannel;
	
	rollChannel = indv_holds & AP_MODE_INDV_MASK_ROLL;
	pitchChannel = indv_holds & AP_MODE_INDV_MASK_PITCH;
	bool cmdRoll=true;
	bool cmdPitch=true;
	bool cmdThro=true;
	bool cmdSlip=true;
	
	if (rollChannel == 0)
		{cmdRoll=false;}
	if (rollChannel >= AP_MODE_INDV_WING_LEVELER) // 1
		{pid.rollHold.run(aircraft->sensors.getRoll(),fcs_roll,pidTarget.roll);}
	if (rollChannel >= AP_MODE_INDV_HEADING) // 2
	{
		double error;
		error = Coord::angleTo(aircraft->sensors.getCourse(),pidTarget.heading);
		pid.headingHold.run(error,pidTarget.roll,0);
	}
	if (pitchChannel == 0)
		{cmdPitch=false;}
	if (pitchChannel >= AP_MODE_INDV_PITCH) // 3
		{pid.pitchHold.run(aircraft->sensors.getPitch(),fcs_pitch,pidTarget.pitch);}
	if (pitchChannel >= AP_MODE_INDV_VRATE) // 4
		{pid.vrateHold.run(aircraft->sensors.getVrate(),pidTarget.pitch,pidTarget.vrate);}
	if (pitchChannel >= AP_MODE_INDV_ALT) // 5
		{pid.altHold.run(aircraft->sensors.getAlt(),pidTarget.vrate,pidTarget.altitude);}
	if (indv_holds & AP_MODE_INDV_THRO) // 6
		{pid.throttleHold.run(aircraft->sensors.getSpeed(),fcs_throttle,pidTarget.speed);}
	else{cmdThro=false;}
	if (indv_holds & AP_MODE_INDV_SLIP) // 7
		{pid.slipHold.run(aircraft->sensors.getSlip(),fcs_yaw,pidTarget.slip);}
	else{cmdSlip=false;}
	
	/*Serial.print("Roll = ");
	Serial.print(aircraft->sensors.position.roll);
	Serial.print("    Target = ");
	Serial.print(pidTarget.roll);
	Serial.print("    FCS_roll = ");
	Serial.println(fcs_roll);*/
	
	if (cmdRoll){aircraft->svoRoll(int(fcs_roll));}
	else{aircraft->RCout(1,aircraft->RCin(1));}
	if (cmdPitch){aircraft->svoPitch(int(fcs_pitch));}
	else{aircraft->RCout(2,aircraft->RCin(2));}
	if (cmdThro){aircraft->throttleCmd(int(fcs_throttle));}
	else{aircraft->RCout(3,aircraft->RCin(3));}
	if (cmdSlip){aircraft->svoRudder(int(fcs_yaw));}
	else{aircraft->RCout(4,aircraft->RCin(4));}
}

void Ap::runWpNav(double dt)
{
	//@TODO
	
	//Update all PIDs
	/*pid.rollHold.run();
	pid.headingHold.run();
	pid.altHold.run();
	pid.vrateHold.run();
	pid.pitchHold.run();
	pid.throttleHold.run();
	pid.slipHold.run();*/
}

void Ap::runLand(double dt)
{
	//@TODO
}

void Ap::offRun()
{
	en = false;
	
	//Null out the pids
	pid.rollHold.nullOut();
	pid.headingHold.nullOut();
	pid.pitchHold.nullOut();
	pid.vrateHold.nullOut();
	pid.altHold.nullOut();
	pid.throttleHold.nullOut();
	pid.slipHold.nullOut();
	
	//Run the 4 channels directly off the inputs
	//aircraft->runOffInputs();
}

//Settings Handlers {
bool Ap::switchToMode(int mode)
{
	if (mode>3){return false;}
	this->mode = mode;
	return true;
}
bool Ap::switchIndvMode(int mode)
{
	if ((mode & 3) == 3 || (mode&0x80)!=0){return false;}
	indv_holds = mode;
	return true;
}
int Ap::getMode(){return mode;}
int Ap::getIndvMode(){return indv_holds;}
void Ap::indv_roll()
{
	mode &= ~AP_MODE_INDV_MASK_ROLL;
	mode |= AP_MODE_INDV_WING_LEVELER;
}
void Ap::indv_heading()
{
	mode &= ~AP_MODE_INDV_MASK_ROLL;
	mode |= AP_MODE_INDV_VRATE;
}
void Ap::indv_rollOff()
{
	mode &= ~AP_MODE_INDV_MASK_ROLL;
	//mode |= OFF; // Does nothing
}
void Ap::indv_pitch()
{
	mode &= ~AP_MODE_INDV_MASK_PITCH;
	mode |= AP_MODE_INDV_PITCH;
}
void Ap::indv_vrate()
{
	mode &= ~AP_MODE_INDV_MASK_PITCH;
	mode |= AP_MODE_INDV_VRATE;
}
void Ap::indv_alt()
{
	mode &= ~AP_MODE_INDV_MASK_PITCH;
	mode |= AP_MODE_INDV_ALT;
}
void Ap::indv_pitchOff()
{
	mode &= ~AP_MODE_INDV_MASK_PITCH;
	//mode |= OFF; // Does nothing
}
void Ap::indv_thro()
{
	//mode &= ~AP_MODE_INDV_MASK_THRO; // Does nothing compared to next command
	mode |= AP_MODE_INDV_THRO;
}
void Ap::indv_throOff()
{
	mode &= AP_MODE_INDV_MASK_THRO;
	//mode |= OFF; // Does nothing
}
void Ap::indv_slip()
{
	//mode &= ~AP_MODE_INDV_MASK_SLIP; // Does nothing compared to next command
	mode |= AP_MODE_INDV_SLIP;
}
void Ap::indv_slipOff()
{
	mode &= ~AP_MODE_INDV_MASK_SLIP;
	//mode |= OFF; // Does nothing
}
bool Ap::setTargetRoll(double roll)
{
	if ((indv_holds&AP_MODE_INDV_MASK_ROLL) > AP_MODE_INDV_WING_LEVELER)
	{return false;} // Cannot apply setting since other modules are using it
	pidTarget.roll = roll;
	return true;
}
bool Ap::setTargetHeading(double heading)
{
	//if ((indv_holds&AP_MODE_INDV_MASK_ROLL) > AP_MODE_INDV_HEADING)
	//{return false;} // Impossible to achieve at the current time
	pidTarget.heading = heading;
	return true;
}
bool Ap::setTargetPitch(double pitch)
{
	if ((indv_holds&AP_MODE_INDV_MASK_PITCH) > AP_MODE_INDV_PITCH)
	{return false;} // Another loop is handling this
	pidTarget.pitch = pitch;
	return true;
}
bool Ap::setTargetVRate(double vrate)
{
	if ((indv_holds&AP_MODE_INDV_MASK_PITCH) > AP_MODE_INDV_VRATE)
	{return false;} // Another loop is handling this
	pidTarget.vrate = vrate;
	return true;
}
bool Ap::setTargetAlt(double alt)
{
	//if ((indv_holds&AP_MODE_INDV_MASK_PITCH) > AP_MODE_INDV_VRATE)
	//{return false;} // Impossible to achieve at this time
	pidTarget.altitude = alt;
	return true;
}
bool Ap::setTargetThrottle(double throttle)
{
	pidTarget.speed = throttle;
	return true;
}
bool Ap::setTargetSlip(double slip)
{
	pidTarget.slip = slip;
	return true;
}

double Ap::getTargetRoll(){return pidTarget.roll;}
double Ap::getTargetHeading(){return pidTarget.heading;}
double Ap::getTargetPitch(){return pidTarget.pitch;}
double Ap::getTargetVRate(){return pidTarget.vrate;}
double Ap::getTargetAlt(){return pidTarget.altitude;}
double Ap::getTargetThrottle(){return pidTarget.speed;}
double Ap::getTargetSlip(){return pidTarget.slip;}

bool Ap::addWp(WP newWp)
{
	wp.push_back(newWp);
	return true;
}
bool Ap::addWp(double lat, double lon, double alt)
{
	wp.push_back(WP(lat,lon,alt));
	return true;
}
bool Ap::rmWpId(unsigned int id)
{
	int index;
	index = findWpIndex(id);
	if (index==-1){return false;}
	wp.erase(wp.begin()+index);
	return true;
}
bool Ap::mvWpIdUnder(unsigned int idFrom, unsigned int idTo)
{
	int from,to;
	from = findWpIndex(idFrom);
	if (from==-1){return false;}
	to = findWpIndex(idFrom);
	if (to==-1){return false;}
	WP t = wp[from];
	wp.erase(wp.begin()+from);
	wp.insert(wp.begin()+to+1);
	return true;
}
bool Ap::mvWpIdOver(unsigned int idFrom, unsigned int idTo)
{
	int from,to;
	from = findWpIndex(idFrom);
	if (from==-1){return false;}
	to = findWpIndex(idFrom);
	if (to==-1){return false;}
	WP t = wp[from];
	wp.erase(wp.begin()+from);
	wp.insert(wp.begin()+to);
	return true;
}
bool Ap::rmWp(unsigned int index)
{
	if (wp.size()<index){return false;}
	wp.erase(wp.begin()+index);
    return true;
}
bool Ap::mvWp(unsigned int indexFrom, unsigned int indexTo)
{
	if (indexFrom >= wp.size() || indexTo >= wp.size()){return false;}
	WP t = wp[indexFrom];
	wp.erase(wp.begin()+indexFrom);
	wp.insert(wp.begin()+indexTo,t);
	return true;
}
std::vector<WP> Ap::getWpList()
{
	return wp;
}
//} Settings Handlers

int Ap::findWpIndex(unsigned int id)
{
	for (int i=0;i<wp.size();i++)
	{
		if (wp[i].getId() == id){return i;}
	}
	return -1; // Not found
}

bool Ap::isEn(){return en;}

void Ap::savePIDtoEEPROM()
{
	SpidConstants pids;
	pid.rollHold.copy(pids.wingLeveler);
	pid.headingHold.copy(pids.headingHold);
	pid.pitchHold.copy(pids.pitchHold);
	pid.vrateHold.copy(pids.climbHold);
	pid.altHold.copy(pids.altHold);
	pid.throttleHold.copy(pids.throttleHold);
	pid.slipHold.copy(pids.slipHold);
	EEPROMmanager::writePIDconstants(pids);
}
